#ifndef GMM_VOXEL_MAP_H
#define GMM_VOXEL_MAP_H

#include <Eigen/Core>
#include <gmm_voxel_map/gmm.h>
#include <gmm_voxel_map/gmm_voxel_map_base.h>
#include <iostream>
#include <ros/ros.h>
#include <unordered_set>
#include <vector>

namespace gvm {
class GMMVoxelMapCircularBuffer {
public:
  GMMVoxelMapCircularBuffer() {}
  ~GMMVoxelMapCircularBuffer() {}
  GMMVoxelMapCircularBuffer(double resolution, double truncation_distance,
                            const std::vector<int> &POW,
                            const std::vector<double> &centre_ratio)
      : N_AXIS({1 << POW[0], 1 << POW[1], 1 << POW[2]}),
        N_MAX(std::max({N_AXIS[0], N_AXIS[1], N_AXIS[2]})),
        resolution_(resolution), truncation_distance_(truncation_distance),
        occupancy_buffer_(resolution, 0.0, POW, centre_ratio),
        edt_buffer_(resolution, truncation_distance, POW, centre_ratio),
        tmp_buffer1_(resolution, truncation_distance, POW, centre_ratio),
        tmp_buffer2_(resolution, truncation_distance, POW, centre_ratio) {
    trueProbability = 0.75; // p(z=1|s=1)=p(z=0|s=0)=trueProbability
    falseProbability =
        1 - trueProbability; // p(z=0|s=1)=p(z=1|s=0)=falseProbability
    lgHit =
        log(trueProbability / falseProbability); // log(p(z=1|s=1)/p(z=1|s=0))
    lgMiss =
        log(falseProbability / trueProbability); // log(p(z=0|s=1)/p(z=0|s=0))
    occThreshold = 3.476;                        // 97% occ probablity
    freeThreshold = -2.9;                        // 5%  occ probablity
    occlimit = occThreshold * 1.5;
    freelimit = freeThreshold * 1.5;
    std::cout << "lgHit,lgMiss,occThreshold,freeThreshold: " << lgHit << ","
              << lgMiss << "," << occThreshold << "," << freeThreshold
              << std::endl;
    ResetEdtRange();
    CalEdgePoint(2.0,2*2.0*tan(20.0/2.0/57.3),2*2.0*tan(20.0/2.0/57.3));
  }
  void SetMapTrueProbability(double SET_MAP_TRUE_PROBABILITY){
    trueProbability = SET_MAP_TRUE_PROBABILITY; // p(z=1|s=1)=p(z=0|s=0)=trueProbability
    falseProbability =
        1 - trueProbability; // p(z=0|s=1)=p(z=1|s=0)=falseProbability
    lgHit =
        log(trueProbability / falseProbability); // log(p(z=1|s=1)/p(z=1|s=0))
    lgMiss =
        log(falseProbability / trueProbability); // log(p(z=0|s=1)/p(z=0|s=0))
    occThreshold = 3.476;                        // 97% occ probablity
    freeThreshold = -2.9;                        // 5%  occ probablity
    occlimit = occThreshold * 1.5;
    freelimit = freeThreshold * 1.5;
    std::cout << "lgHit,lgMiss,occThreshold,freeThreshold: " << lgHit << ","
              << lgMiss << "," << occThreshold << "," << freeThreshold
              << std::endl;
  }
  void SetFov(double range_distance, double horizontal_fov, double vertical_fov){
    CalEdgePoint(range_distance,2*range_distance*tan(horizontal_fov/2.0/57.3),2*range_distance*tan(vertical_fov/2.0/57.3));
  }
  std::vector<int> N_AXIS;
  int N_MAX;
  double trueProbability;
  double falseProbability;
  double lgHit;
  double lgMiss;
  double occThreshold;
  double freeThreshold;
  double occlimit;
  double freelimit;
  double resolution_;
  double truncation_distance_;
  VoxelMapCircularBufferBase<double> occupancy_buffer_;
  VoxelMapCircularBufferBase<double> edt_buffer_;
  Eigen::Vector3i updated_minIdx_; // Idx is local idx
  Eigen::Vector3i updated_maxIdx_; // Idx is local idx
  VoxelMapCircularBufferBase<int> tmp_buffer1_;
  VoxelMapCircularBufferBase<int> tmp_buffer2_;
  std::unordered_set<int> recordedPoint;
  std::vector<Eigen::Vector3d> edge_point_;

private:
  void CalEdgePoint(double range_distance, double edge_width,
                    double edge_height) {
    for (double j = 0.5 * edge_height; j >= -0.5 * edge_height;
         j -= resolution_) {
      for (double i = 0.5 * edge_width; i >= -0.5 * edge_width; i -= resolution_) {
        edge_point_.emplace_back(Eigen::Vector3d(range_distance, i, j));
      }
    }    
  }
  template <typename T> void MyPrint(std::string s, const T &eigen_vector) {
    std::cout << s << eigen_vector(0) << " , " << eigen_vector(1) << " , "
              << eigen_vector(2) << std::endl;
  }
  inline void ResetEdtRange() {
    updated_minIdx_ << 9999, 9999, 9999;
    updated_maxIdx_ << -9999, -9999, -9999;
  }
  void UpdateEdtRange(const Eigen::Vector3i &index) {
    updated_minIdx_ = updated_minIdx_.array().min(index.array());
    updated_maxIdx_ = updated_maxIdx_.array().max(index.array());
    updated_maxIdx_.array() = updated_maxIdx_.array().min(
        Eigen::Vector3i(N_AXIS[0] - 1, N_AXIS[1] - 1, N_AXIS[2] - 1).array());
    updated_minIdx_.array() =
        updated_minIdx_.array().max(Eigen::Vector3i(0, 0, 0).array());
  }
  void CheckNewBoundary(const Eigen::Vector3i &index) {
    Eigen::Vector3i max_update_idx =
        index + static_cast<int>(truncation_distance_ / resolution_) *
                    Eigen::Vector3i(1, 1, 1);
    Eigen::Vector3i min_update_idx =
        index - static_cast<int>(truncation_distance_ / resolution_) *
                    Eigen::Vector3i(1, 1, 1);
    max_update_idx.array() = max_update_idx.array().min(
        Eigen::Vector3i(N_AXIS[0] - 1, N_AXIS[1] - 1, N_AXIS[2] - 1).array());
    min_update_idx.array() =
        min_update_idx.array().max(Eigen::Vector3i(0, 0, 0).array());
    Eigen::Vector3i offset = edt_buffer_.GetOffset();
    max_update_idx += offset;
    min_update_idx += offset;
    if (!insideMapRange(max_update_idx) ||
        !insideMapRange(Eigen::Vector3i(min_update_idx(0), max_update_idx(1),
                                        max_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(min_update_idx(0), min_update_idx(1),
                                        max_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(max_update_idx(0), min_update_idx(1),
                                        max_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(max_update_idx(0), max_update_idx(1),
                                        min_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(min_update_idx(0), max_update_idx(1),
                                        min_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(min_update_idx(0), min_update_idx(1),
                                        min_update_idx(2))) ||
        !insideMapRange(Eigen::Vector3i(max_update_idx(0), min_update_idx(1),
                                        min_update_idx(2)))) {
      UpdateEdtRange(index);
    }
  }
  // index is a local map index
  void addHit(const Eigen::Vector3i &index) {
    bool last_occupied = getMapLocalIdxOccupied(index);
    occupancy_buffer_.at(index) = occupancy_buffer_.at(index) + lgHit;
    if (occupancy_buffer_.at(index) >= occlimit)
      occupancy_buffer_.at(index) = occlimit;
    bool new_occupied = getMapLocalIdxOccupied(index);
    if (last_occupied != new_occupied) {
      UpdateEdtRange(index);
      return;
    }
    CheckNewBoundary(index);
  }

  void addMiss(const Eigen::Vector3i &index) {
    bool last_occupied = getMapLocalIdxOccupied(index);
    occupancy_buffer_.at(index) = occupancy_buffer_.at(index) + lgMiss;
    if (occupancy_buffer_.at(index) <= freelimit)
      occupancy_buffer_.at(index) = freelimit;
    bool new_occupied = getMapLocalIdxOccupied(index);
    if (last_occupied != new_occupied) {
      UpdateEdtRange(index);
      return;
    }
    // CheckNewBoundary(index);
  }

  template <typename F_get_val, typename F_set_val>
  void fill_edt(F_get_val f_get_val, F_set_val f_set_val, int start, int end) {
    int v[N_MAX];
    float z[N_MAX + 1];
    int k = start;
    v[start] = start;
    z[start] = -99999;
    z[start + 1] = 99999;
    for (int q = start + 1; q <= end; q++) {
      k++;
      float s;
      do {
        k--;
        s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) /
            (2 * q - 2 * v[k]);
        // ROS_INFO_STREAM("k: " << k << " s: " <<  s  << " z[k] " << z[k] << "
        // v[k] " << v[k]);

      } while (s <= z[k]);
      k++;
      v[k] = q;
      z[k] = s;
      z[k + 1] = 99999;
    }
    k = start;
    for (int q = start; q <= end; q++) {
      while (z[k + 1] < q)
        k++;
      float val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
      //      if(val < std::numeric_limits<_Scalar>::max())
      //  ROS_INFO_STREAM("val: " << val << " q: " << q << " v[k] " << v[k]);
      // if(val > truncation_distance_*truncation_distance_) val =
      // std::numeric_limits<_Scalar>::max();
      f_set_val(q, val);
    }
  }
  // pos is global pos
  void getSurroundPts(const Eigen::Vector3d &pos, Eigen::Vector3d pts[2][2][2],
                      Eigen::Vector3d &diff) {
    /* interpolation position */
    Eigen::Vector3d pos_m = pos - 0.5 * resolution_ * Eigen::Vector3d::Ones();
    Eigen::Vector3i idx;
    Eigen::Vector3d idx_pos;
    getmapIdxLocal(pos_m, idx);
    idx_pos = getMapGlobalPosfromLocalIdx(idx);
    diff = (pos - idx_pos) / resolution_;

    for (int x = 0; x < 2; x++) {
      for (int y = 0; y < 2; y++) {
        for (int z = 0; z < 2; z++) {
          Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z);
          Eigen::Vector3d current_pos;
          current_pos = getMapGlobalPosfromLocalIdx(current_idx);
          pts[x][y][z] = current_pos;
        }
      }
    }
  }

  void getSurroundDistance(Eigen::Vector3d pts[2][2][2],
                           double dists[2][2][2]) {
    for (int x = 0; x < 2; x++) {
      for (int y = 0; y < 2; y++) {
        for (int z = 0; z < 2; z++) {
          // ROS_INFO("pts[x][y][z]: %f %f %f",
          // pts[x][y][z](0),pts[x][y][z](1),pts[x][y][z](2));
          dists[x][y][z] = getMapGlobalPosEdt(pts[x][y][z]);
        }
      }
    }
  }

  void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d &diff,
                            double &value, Eigen::Vector3d &grad) {
    // trilinear interpolation
    double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
    double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
    double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
    double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
    double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
    double v1 = (1 - diff(1)) * v01 + diff(1) * v11;
    value = (1 - diff(2)) * v0 + diff(2) * v1;
    grad[2] = (v1 - v0) / resolution_;
    grad[1] =
        ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) / resolution_;
    grad[0] =
        (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
    grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
    grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
    grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
    double resolution_inv_ = 1 / resolution_;
    grad[0] *= resolution_inv_;
  }

  void insertFreeBresenham3D(const Eigen::Vector3i &point_idx,
                             const Eigen::Vector3i &origin_idx,
                             std::vector<Eigen::Vector3i> &bresenhamPath) {
    bresenhamPath.push_back(origin_idx);
    int dx = abs(point_idx(0) - origin_idx(0));
    int dy = abs(point_idx(1) - origin_idx(1));
    int dz = abs(point_idx(2) - origin_idx(2));
    int xs, ys, zs;
    xs = (point_idx(0) > origin_idx(0)) ? 1 : -1;
    ys = (point_idx(1) > origin_idx(1)) ? 1 : -1;
    zs = (point_idx(2) > origin_idx(2)) ? 1 : -1;
    int x1, y1, z1, x2, y2, z2;
    x1 = origin_idx(0);
    y1 = origin_idx(1);
    z1 = origin_idx(2);
    x2 = point_idx(0);
    y2 = point_idx(1);
    z2 = point_idx(2);
    int p1, p2;
    // # Driving axis is X-axis"
    if (dx >= dy && dx >= dz) {
      p1 = 2 * dy - dx;
      p2 = 2 * dz - dx;
      while (x1 + xs != x2) {
        x1 += xs;
        if (p1 >= 0) {
          y1 += ys;
          p1 -= 2 * dx;
        }

        if (p2 >= 0) {
          z1 += zs;
          p2 -= 2 * dx;
        }
        p1 += 2 * dy;
        p2 += 2 * dz;
        Eigen::Vector3i tmp(x1, y1, z1);
        bresenhamPath.push_back(tmp);
      }
    }
    // # Driving axis is Y-axis"
    else if (dy >= dx and dy >= dz) {
      p1 = 2 * dx - dy;
      p2 = 2 * dz - dy;
      while (y1 + ys != y2) {
        y1 += ys;
        if (p1 >= 0) {
          x1 += xs;
          p1 -= 2 * dy;
        }
        if (p2 >= 0) {
          z1 += zs;
          p2 -= 2 * dy;
        }
        p1 += 2 * dx;
        p2 += 2 * dz;
        Eigen::Vector3i tmp(x1, y1, z1);
        bresenhamPath.push_back(tmp);
      }
    }
    // # Driving axis is Z-axis"
    else {
      p1 = 2 * dy - dz;
      p2 = 2 * dx - dz;
      while (z1 + zs != z2) {
        z1 += zs;
        if (p1 >= 0) {
          y1 += ys;
          p1 -= 2 * dz;
        }
        if (p2 >= 0) {
          x1 += xs;
          p2 -= 2 * dz;
        }
        p1 += 2 * dy;
        p2 += 2 * dx;
        Eigen::Vector3i tmp(x1, y1, z1);
        bresenhamPath.push_back(tmp);
      }
    }
  }

public:
  // coordinate is the global position. index is a local map idx in which the
  // centre idx is constant.
  void getmapIdxLocal(const Eigen::Vector3d &coordinate,
                      Eigen::Vector3i &index) {
    index = occupancy_buffer_.getIdxLocal(coordinate);
  }
  inline double getMapGlobalPosEdt(const Eigen::Vector3d &pos) {
    // pos is the global position
    return edt_buffer_.getPosOccupiedProb(pos);
  }
  inline double getMapLocalIdxEdt(const Eigen::Vector3i &idx) {
    return edt_buffer_.getLocalIdxOccupiedProb(idx);
  }
  bool insideMapRange(const Eigen::Vector3i &idx) {
    return occupancy_buffer_.insideRange(idx);
  }
  inline void setMapInitPose(const Eigen::Vector3d &pos) {
    occupancy_buffer_.mapCentrePos = pos;
    edt_buffer_.mapCentrePos = pos;
  }
  inline int indexRange(int axis) { return occupancy_buffer_.N_AXIS[axis]; }
  inline int indexCentre(int axis) { return occupancy_buffer_.N_CENTRE[axis]; }
  // idx is locbal index
  bool getMapLocalIdxOccupied(const Eigen::Vector3i &idx) {
    double prob = occupancy_buffer_.getLocalIdxOccupiedProb(idx);
    return prob >= occThreshold;
  }
  bool getMapGlobalPosOccupied(const Eigen::Vector3d &pos) {
    double prob = occupancy_buffer_.getPosOccupiedProb(pos);
    return prob >= occThreshold;
  }
  Eigen::Vector3d getMapGlobalPosfromLocalIdx(const Eigen::Vector3i &idx) {
    return occupancy_buffer_.getGlobalPosfromLocalIdx(idx);
  }
  void updateOrigin(const Eigen::Vector3d &new_originPos) {
    occupancy_buffer_.updateMapOrigin(new_originPos);
    edt_buffer_.updateMapOrigin(new_originPos);
  }

  template <typename T_PC>
  void updateGridMapFromPointCLoud(const T_PC &pointcloud) {
    ResetEdtRange();
    Eigen::Vector3i origin_index = occupancy_buffer_.mapCentreIdx;
    for (auto it : pointcloud->points) {
      Eigen::Vector3d point(it.x, it.y, it.z);
      Eigen::Vector3i map_index;
      getmapIdxLocal(point, map_index);
      bool insideFlag = true;
      if (!insideMapRange(map_index)) {
        insideFlag = false;
        map_index = occupancy_buffer_.getClosestIdxInRange(
            occupancy_buffer_.mapCentrePos, point);
        if (!insideMapRange(map_index)) {
          ROS_ERROR("sample point out of range!!!!!!!!!");
          continue;
        }
      }
      if (insideFlag)
        addHit(map_index);
      else
        addMiss(map_index);
      std::vector<Eigen::Vector3i> bresenhamPath;
      if (map_index == origin_index)
        continue;
      insertFreeBresenham3D(map_index, origin_index, bresenhamPath);
      for (std::vector<Eigen::Vector3i>::iterator iter_bresenhamPath =
               bresenhamPath.begin();
           iter_bresenhamPath != bresenhamPath.end(); iter_bresenhamPath++) {
        // ROS_INFO("bressenham point: %d %d
        // %d",(*iter_bresenhamPath)(0),(*iter_bresenhamPath)(1),(*iter_bresenhamPath)(2));
        addMiss(*iter_bresenhamPath);
      }
    }
  }

  void MCRayTracing(const std::vector<Eigen::Vector3d> &samplePoints) {
    ResetEdtRange();
    Eigen::Vector3i origin_index = occupancy_buffer_.mapCentreIdx;
    Eigen::Vector3i map_index;
    for (auto iter = samplePoints.begin(); iter != samplePoints.end(); iter++) {
      getmapIdxLocal(*iter, map_index);
      // Eigen::Vector3d global_pos = getMapGlobalPosfromLocalIdx(map_index);
      // std::cout << "orgin_point: " << iter->x() << " " << iter->y() << " "
      //           << iter->z() << " map_idx: " << map_index(0) << " "
      //           << map_index(1) << " " << map_index(2)
      //           << " map pos: " << global_pos(0) << " " << global_pos(1) << "
      //           "
      //           << global_pos(2) << std::endl;
      bool insideFlag = true;
      if (!insideMapRange(map_index)) {
        insideFlag = false;
        map_index = occupancy_buffer_.getClosestIdxInRange(
            occupancy_buffer_.mapCentrePos, *iter);
        if (!insideMapRange(map_index)) {
          ROS_ERROR("sample point out of range!!!!!!!!!");
          continue;
        }
      }
      if (insideFlag)
        addHit(map_index);
      else
        addMiss(map_index);
      std::vector<Eigen::Vector3i> bresenhamPath;
      if (map_index == origin_index)
        continue;
      insertFreeBresenham3D(map_index, origin_index, bresenhamPath);
      for (std::vector<Eigen::Vector3i>::iterator iter_bresenhamPath =
               bresenhamPath.begin();
           iter_bresenhamPath != bresenhamPath.end(); iter_bresenhamPath++) {
        // ROS_INFO("bressenham point: %d %d
        // %d",(*iter_bresenhamPath)(0),(*iter_bresenhamPath)(1),(*iter_bresenhamPath)(2));
        addMiss(*iter_bresenhamPath);
      }
    }
  }

  void MCRayTracing(const GMM &gmm, double n_sigma, bool map_add_miss) {
    ResetEdtRange();
    recordedPoint.clear();
    int samplenum = 200;
    std::vector<Eigen::Vector3d> samplePoints =
        gmm.GMMRandomSamplePoints( n_sigma);
    Eigen::Vector3i origin_index;
    getmapIdxLocal(gmm.sensor_location_, origin_index);
    Eigen::Vector3i map_index;
    for (auto iter = samplePoints.begin(); iter != samplePoints.end(); iter++) {
      getmapIdxLocal(*iter, map_index);
      bool insideFlag = true;
      if (!insideMapRange(map_index)) {
        insideFlag = false;
        map_index =
            occupancy_buffer_.getClosestIdxInRange(gmm.sensor_location_, *iter);
        if (!insideMapRange(map_index)) {
          std::cout << "map_index: " << map_index(0) << "," << map_index(1)
                    << "," << map_index(2) << std::endl;
          ROS_ERROR("sample point out of range!!!!!!!!!");
          continue;
        }
      }
      if (insideFlag) {
        addHit(map_index);
        recordedPoint.insert(map_index(0) + map_index(1) * N_AXIS[0] +
                             map_index(2) * N_AXIS[0] * N_AXIS[1]);
      } else{
        // addMiss(map_index);
      }
      if (map_index == origin_index)
        continue;
      if(!map_add_miss)
        continue;
      std::vector<Eigen::Vector3i> bresenhamPath;
      insertFreeBresenham3D(map_index, origin_index, bresenhamPath);
      for (auto iter_bresenhamPath = bresenhamPath.begin();
           iter_bresenhamPath != bresenhamPath.end(); iter_bresenhamPath++) {
        addMiss(*iter_bresenhamPath);
        recordedPoint.insert(map_index(0) + map_index(1) * N_AXIS[0] +
                             map_index(2) * N_AXIS[0] * N_AXIS[1]);
      }
    }
  }

  void UpdateEdgePoints(const Eigen::Vector3d &current_p,
                        const Eigen::Quaterniond &current_q) {
    Eigen::Vector3i origin_index;
    getmapIdxLocal(current_p, origin_index);
    for (int i = 0; i < edge_point_.size(); i++) {
      Eigen::Vector3d transfer_edge_point;
      transfer_edge_point = current_q * edge_point_[i] + current_p;
      Eigen::Vector3i transfer_edge_point_idx;
      getmapIdxLocal(transfer_edge_point,transfer_edge_point_idx);
      std::vector<Eigen::Vector3i> bre_path;
      insertFreeBresenham3D(transfer_edge_point_idx, origin_index, bre_path);
      for(int i=0;i<bre_path.size();i++){
        if(recordedPoint.count(bre_path[i](0) + bre_path[i](1) * N_AXIS[0] +
                             bre_path[i](2) * N_AXIS[0] * N_AXIS[1])){
          break;
        }else{
          addMiss(bre_path[i]);
        }
      }
    }
  }

  void updateEDTMap() {
    if (updated_minIdx_(0) > updated_maxIdx_(0)) // no update voxel
      return;
    Eigen::Vector3i edtUpdate_minIdx = updated_minIdx_;
    Eigen::Vector3i edtUpdate_maxIdx = updated_maxIdx_;
    edtUpdate_minIdx.array() -= (truncation_distance_) / resolution_;
    edtUpdate_maxIdx.array() += (truncation_distance_) / resolution_;
    edtUpdate_minIdx.array() =
        edtUpdate_minIdx.array().max(Eigen::Vector3i(0, 0, 0).array());
    edtUpdate_maxIdx.array() = edtUpdate_maxIdx.array().min(
        Eigen::Vector3i(N_AXIS[0] - 1, N_AXIS[1] - 1, N_AXIS[2] - 1).array());
    updated_minIdx_.array() -= (truncation_distance_ + 0.7) / resolution_;
    updated_maxIdx_.array() += (truncation_distance_ + 0.7) / resolution_;
    updated_minIdx_.array() =
        updated_minIdx_.array().max(Eigen::Vector3i(0, 0, 0).array());
    updated_maxIdx_.array() = updated_maxIdx_.array().min(
        Eigen::Vector3i(N_AXIS[0] - 1, N_AXIS[1] - 1, N_AXIS[2] - 1).array());
    // ROS_INFO("updated_minIdx_: %d %d
    // %d",updated_minIdx_(0),updated_minIdx_(1),updated_minIdx_(2));
    // ROS_INFO("updated_maxIdx_: %d %d
    // %d",updated_maxIdx_(0),updated_maxIdx_(1),updated_maxIdx_(2));
    ros::Time firstloopstart = ros::Time::now();
    for (int x = updated_minIdx_(0); x <= updated_maxIdx_(0);
         x++) // x, y plane search
    {
      for (int y = updated_minIdx_(1); y <= updated_maxIdx_(1); y++) {
        fill_edt(
            [&](int z) {
              return getMapLocalIdxOccupied(Eigen::Vector3i(x, y, z)) ? 0
                                                                      : 9999;
            },
            [&](int z, float val) {
              tmp_buffer1_.at(Eigen::Vector3i(x, y, z)) = val;
            },
            updated_minIdx_(2), updated_maxIdx_(2));
      }
    }
    ros::Time firstloopend = ros::Time::now();
    // ROS_INFO("FIRST TIME USE: %f",(firstloopend-firstloopstart).toSec());

    for (int x = updated_minIdx_(0); x <= updated_maxIdx_(0);
         x++) // x, z plane search
    {
      for (int z = updated_minIdx_(2); z <= updated_maxIdx_(2); z++) {
        fill_edt(
            [&](int y) { return tmp_buffer1_.at(Eigen::Vector3i(x, y, z)); },
            [&](int y, float val) {
              tmp_buffer2_.at(Eigen::Vector3i(x, y, z)) = val;
            },
            updated_minIdx_(1), updated_maxIdx_(1));
      }
    }

    for (int y = updated_minIdx_(1); y <= updated_maxIdx_(1);
         y++) // y, z plane search
    {
      for (int z = updated_minIdx_(2); z <= updated_maxIdx_(2); z++) {
        fill_edt(
            [&](int x) { return tmp_buffer2_.at(Eigen::Vector3i(x, y, z)); },
            [&](int x, float val) {
              Eigen::Vector3i tmp(x, y, z);
              if (x >= edtUpdate_minIdx(0) && x <= edtUpdate_maxIdx(0) &&
                  y >= edtUpdate_minIdx(1) && y <= edtUpdate_maxIdx(1) &&
                  z >= edtUpdate_minIdx(2) && z <= edtUpdate_maxIdx(2))
                edt_buffer_.at(Eigen::Vector3i(x, y, z)) =
                    std::min(resolution_ * std::sqrt(val),
                             truncation_distance_); // Set final offset
            },
            updated_minIdx_(0), updated_maxIdx_(0));
      }
    }
    ResetEdtRange();
  }
  // get distance and gradient at pos, pos is a global position
  void evaluateEDTWithGrad(const Eigen::Vector3d &pos, double time,
                           double &dist, Eigen::Vector3d &grad) {
    Eigen::Vector3d diff;
    Eigen::Vector3d sur_pts[2][2][2];
    double dists[2][2][2];
    getSurroundPts(pos, sur_pts, diff);
    getSurroundDistance(sur_pts, dists);
    interpolateTrilinear(dists, diff, dist, grad);
  }

  // idx is locbal index
  double getMapLocalIdxEntropy(const Eigen::Vector3i &idx) {
    double prob = occupancy_buffer_.getLocalIdxOccupiedProb(idx);
    prob = exp(prob) / (exp(prob) + 1);
    return -prob * log(prob + 0.001);
  }

  // idx is local map idx
  Eigen::Vector3i getLocalIdxMapEdtColor(const Eigen::Vector3i &idx) {
    double distance = getMapLocalIdxEdt(idx);
    int value = floor(distance * 240); // Mapping 0~1.0 to 0~240
    value = value > 240 ? 240 : value;
    int interval = 60;
    // 240 degrees are divided into 4 sections, 0 is Red-dominant, 1 and 2 are
    // Green-dominant, 3 is Blue-dominant. The dorminant color is 255 and
    // another color is always 0 while the remaining color increases or
    // decreases between 0~255
    int section = value / interval;
    float float_key = (value % interval) / (float)interval * 255;
    int key = floor(float_key);
    int nkey = 255 - key;
    Eigen::Vector3i point_RGB;
    switch (section) {
    case 0: // G increase
      point_RGB(0) = 255;
      point_RGB(1) = key;
      point_RGB(2) = 0;
      break;
    case 1: // R decrease
      point_RGB(0) = nkey;
      point_RGB(1) = 255;
      point_RGB(2) = 0;
      break;
    case 2: // B increase
      point_RGB(0) = 0;
      point_RGB(1) = 255;
      point_RGB(2) = key;
      break;
    case 3: // G decrease
      point_RGB(0) = 0;
      point_RGB(1) = nkey;
      point_RGB(2) = 255;
      break;
    case 4:
      point_RGB(0) = 0;
      point_RGB(1) = 0;
      point_RGB(2) = 255;
      break;
    default: // White
      point_RGB(0) = 255;
      point_RGB(1) = 255;
      point_RGB(2) = 255;
    }
    return point_RGB;
  }
};
} // namespace gvm

#endif
